/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention      : 1. The measure of frequency is based on the input capture mode,
 *                   and it is accurate when the frequency is below 1200Hz
 *                   2. The function putFloat() can be seen in https://blog.csdn.net/jianfeng_zhang1990/article/details/45395987
 *
 * Copyright (c) 2024 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "tim.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdint.h"
#include "string.h"
#include "stdlib.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
uint32_t testNum = 0;
uint8_t channelID = 0;

// Ring Queue
#define QUEUE_SIZE 12
typedef struct
{
    float freq[QUEUE_SIZE];
    uint8_t front;
    uint8_t rear;
} freqQueue_t;

typedef struct rectangleWave
{                              // Hold the rectangle wave parameters
    uint32_t readCurrentValue; // The return value of HAL_TIM_ReadCapturedValue is uint32_t
    uint32_t readLastValue;
    uint32_t captureValue; // The difference between the second and first value
    uint32_t overCount;    // Count the number of timer overflow
    uint32_t overCountLast;
    float freq;
    freqQueue_t ringQueue;
    freqQueue_t tempRingQueue;
    float duty;    // Not used in this example
    uint32_t txID; // The ID of the transmission frame
    uint8_t txBuf[4];
    uint8_t txFlag; // Transmission flag
} rectangleWave_t;

rectangleWave_t T3CH1 = {0, 0, 0, 0, 0, 0, {{0}, 0, 0}, {{0}, 0, 0}, 0, 0x110, 0, 0}; // Initialize the rectangle wave parameters
rectangleWave_t T3CH2 = {0, 0, 0, 0, 0, 0, {{0}, 0, 0}, {{0}, 0, 0}, 0, 0x111, 0, 0};
rectangleWave_t T3CH3 = {0, 0, 0, 0, 0, 0, {{0}, 0, 0}, {{0}, 0, 0}, 0, 0x112, 0, 0};
rectangleWave_t T3CH4 = {0, 0, 0, 0, 0, 0, {{0}, 0, 0}, {{0}, 0, 0}, 0, 0x113, 0, 0};

float freqBuf[4] = {0, 0, 0, 0};     // Store the frequency of each channel
float lowPassFilterProportion = 0.5; // Low pass filter coefficient
uint32_t waveCutUs = 850;            // The minimum value of the wave width

void putFloat(float floatValue, unsigned char *outBuffer, uint8_t startIdx);
void canFilterInit(void);
uint8_t canSendMsg(uint8_t *msg, uint8_t len, uint32_t txID);
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{
    /* USER CODE BEGIN 1 */

    /* USER CODE END 1 */

    /* MCU Configuration--------------------------------------------------------*/

    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
    HAL_Init();

    /* USER CODE BEGIN Init */
    GPIO_InitTypeDef GPIOC_Init;
    GPIOC_Init.Pin = GPIO_PIN_13;
    GPIOC_Init.Mode = GPIO_MODE_OUTPUT_PP;
    GPIOC_Init.Pull = GPIO_PULLDOWN;
    GPIOC_Init.Speed = GPIO_SPEED_FREQ_LOW;
    /* USER CODE END Init */

    /* Configure the system clock */
    SystemClock_Config();

    /* USER CODE BEGIN SysInit */

    /* USER CODE END SysInit */

    /* Initialize all configured peripherals */
    MX_GPIO_Init();
    MX_TIM3_Init();
    MX_CAN_Init();
    MX_TIM4_Init();
    /* USER CODE BEGIN 2 */
    HAL_GPIO_Init(GPIOC, &GPIOC_Init);
    canFilterInit();
    // HAL_TIM_Base_Start(&htim3); // Attention! This initialization cannot be writen with HAL_TIM_Base_Start_IT(&htim3) at the same time
    HAL_TIM_Base_Start_IT(&htim4);
    HAL_TIM_Base_Start_IT(&htim3);
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_1);
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_2);
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_3);
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_4);
    /* USER CODE END 2 */

    /* Infinite loop */
    /* USER CODE BEGIN WHILE */
    while (1)
    {
        /* USER CODE END WHILE */

        /* USER CODE BEGIN 3 */
    }
    /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    /** Initializes the RCC Oscillators according to the specified parameters
     * in the RCC_OscInitTypeDef structure.
     */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
    RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL6;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /** Initializes the CPU, AHB and APB buses clocks
     */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
    {
        Error_Handler();
    }
}
/* USER CODE BEGIN 4 */
// Handle timer overflow interrupt
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim == &htim3)
    {
        T3CH1.overCount++;
        T3CH2.overCount++;
        T3CH3.overCount++;
        T3CH4.overCount++;
        HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
    }
    if (htim == &htim4)
    {
        testNum++;
        channelID = testNum % 4;

        uint32_t currentValue;
        uint32_t captureValue;
        switch (channelID)
        {
        case 0:
            // Judge if the frequency is 0
            currentValue = HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_1);
            if (currentValue > T3CH1.readLastValue)
            {
                // If there is some overflow
                captureValue = currentValue - T3CH1.readLastValue + 0xFFFF * (T3CH1.overCount - T3CH1.overCountLast);
            }
            else // If there is some overflow
            {
                captureValue = 0xFFFF * (T3CH1.overCount - T3CH1.overCountLast) - T3CH1.readLastValue + currentValue;
            }
            if (captureValue >= 8 * 0xFFFF && T3CH1.freq < 50)
            {
                T3CH1.freq = 0;
                T3CH1.ringQueue.front = 0;
                T3CH1.ringQueue.rear = 0;
            }
            putFloat(T3CH1.freq * 10, T3CH1.txBuf, 0);
            T3CH1.txFlag = canSendMsg(T3CH1.txBuf, 4, T3CH1.txID);
            memset(T3CH1.txBuf, 0, sizeof(T3CH1.txBuf));
            break;
        case 1:
            // Judge if the frequency is 0
            currentValue = HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_2);
            if (currentValue > T3CH2.readLastValue)
            {
                // If there is some overflow
                captureValue = currentValue - T3CH2.readLastValue + 0xFFFF * (T3CH2.overCount - T3CH2.overCountLast);
            }
            else // If there is some overflow
            {
                captureValue = 0xFFFF * (T3CH2.overCount - T3CH2.overCountLast) - T3CH2.readLastValue + currentValue;
            }
            if (captureValue >= 8 * 0xFFFF && T3CH2.freq < 50)
            {
                T3CH2.freq = 0;
                T3CH2.ringQueue.front = 0;
                T3CH2.ringQueue.rear = 0;
            }
            putFloat(T3CH2.freq * 10, T3CH2.txBuf, 0);
            T3CH2.txFlag = canSendMsg(T3CH2.txBuf, 4, T3CH2.txID);
            memset(T3CH2.txBuf, 0, sizeof(T3CH2.txBuf));
            break;
        case 2:
            // Judge if the frequency is 0
            currentValue = HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_3);
            if (currentValue > T3CH3.readLastValue)
            {
                // If there is some overflow
                captureValue = currentValue - T3CH3.readLastValue + 0xFFFF * (T3CH3.overCount - T3CH3.overCountLast);
            }
            else // If there is some overflow
            {
                captureValue = 0xFFFF * (T3CH3.overCount - T3CH3.overCountLast) - T3CH3.readLastValue + currentValue;
            }
            if (captureValue >= 8 * 0xFFFF && T3CH3.freq < 50)
            {
                T3CH3.freq = 0;
                T3CH3.ringQueue.front = 0;
                T3CH3.ringQueue.rear = 0;
            }
            putFloat(T3CH3.freq * 10, T3CH3.txBuf, 0);
            T3CH3.txFlag = canSendMsg(T3CH3.txBuf, 4, T3CH3.txID);
            memset(T3CH3.txBuf, 0, sizeof(T3CH3.txBuf));
            break;
        case 3:
            // Judge if the frequency is 0
            currentValue = HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_4);
            if (currentValue > T3CH4.readLastValue)
            {
                // If there is some overflow
                captureValue = currentValue - T3CH4.readLastValue + 0xFFFF * (T3CH4.overCount - T3CH4.overCountLast);
            }
            else // If there is some overflow
            {
                captureValue = 0xFFFF * (T3CH4.overCount - T3CH4.overCountLast) - T3CH4.readLastValue + currentValue;
            }
            if (captureValue >= 8 * 0xFFFF && T3CH4.freq < 50)
            {
                T3CH4.freq = 0;
                T3CH4.ringQueue.front = 0;
                T3CH4.ringQueue.rear = 0;
            }
            putFloat(T3CH4.freq * 10, T3CH4.txBuf, 0);
            T3CH4.txFlag = canSendMsg(T3CH4.txBuf, 4, T3CH4.txID);
            memset(T3CH4.txBuf, 0, sizeof(T3CH4.txBuf));
            break;
        }
    }
}

void swap(float *a, float *b)
{
    float temp = *a;
    *a = *b;
    *b = temp;
}

int partition(float L[], uint8_t low, uint8_t high)
{
    int i, num = low;
    for (i = low + 1; i <= high; i++)
    {
        if (L[i] < L[low])
        {
            swap(&L[i], &L[num + 1]);
            num++;
        }
    }
    swap(&L[low], &L[num]);
    return num;
}

float getMiddleValue(float *freqBuf, uint8_t low, uint8_t high)
{
    int mid = (low + high) / 2;
    while (1)
    {
        int pos = partition(freqBuf, low, high);
        if (pos == mid)
            break;
        else if (pos > mid)
            high = pos - 1;
        else
            low = pos + 1;
    }
    return freqBuf[mid];
}

void calculateFreq(rectangleWave_t *T3CHx, uint32_t channel, uint32_t overCount)
{
    T3CHx->readCurrentValue = HAL_TIM_ReadCapturedValue(&htim3, channel);
    if (T3CHx->readCurrentValue > T3CHx->readLastValue)
    {
        // If there is some overflow
        T3CHx->captureValue = T3CHx->readCurrentValue - T3CHx->readLastValue + 0xFFFF * (overCount - T3CHx->overCountLast);
    }
    else // If there is some overflow
    {
        T3CHx->captureValue = 0xFFFF * (overCount - T3CHx->overCountLast) - T3CHx->readLastValue + T3CHx->readCurrentValue;
    }

    // Remove the noise, the frequency should be less than 1000000/650 = 1538Hz
    if (T3CHx->captureValue < waveCutUs)
        return;

    float freq = 1000000.0 / T3CHx->captureValue; // Timer frequency is 1MHz

    // Freq high and differ limit
    if (freq > 1500 || freq - T3CHx->freq > 200)
        return;

    // Store the frequency in the ring queue
    T3CHx->ringQueue.freq[T3CHx->ringQueue.rear] = freq;
    T3CHx->ringQueue.rear = (T3CHx->ringQueue.rear + 1) % QUEUE_SIZE;
    // If the ring queue is full, clear the front element and move the front pointer
    if (T3CHx->ringQueue.rear == T3CHx->ringQueue.front) //
    {
        T3CHx->ringQueue.front = (T3CHx->ringQueue.front + 1) % QUEUE_SIZE;
    }

    // Calculate the middle value of the ring queue
    memcpy(T3CHx->tempRingQueue.freq, T3CHx->ringQueue.freq, sizeof(T3CHx->ringQueue.freq));
    freq = getMiddleValue(T3CHx->tempRingQueue.freq, 0, QUEUE_SIZE - 1);

    // Low pass filter
    T3CHx->freq = T3CHx->freq * lowPassFilterProportion + freq * (1 - lowPassFilterProportion);
    // T3CHx->freq = freq;
    T3CHx->duty = (T3CHx->readLastValue - T3CHx->readCurrentValue) / (float)T3CHx->captureValue;
    T3CHx->readLastValue = T3CHx->readCurrentValue;
    T3CHx->overCountLast = overCount;
}

// Handle input capture interrupt
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim == &htim3)
    {
        if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
        {
            calculateFreq(&T3CH1, TIM_CHANNEL_1, T3CH1.overCount);
        }
        else if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2)
        {
            calculateFreq(&T3CH2, TIM_CHANNEL_2, T3CH2.overCount);
        }
        else if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_3)
        {
            calculateFreq(&T3CH3, TIM_CHANNEL_3, T3CH3.overCount);
        }
        else if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_4)
        {
            calculateFreq(&T3CH4, TIM_CHANNEL_4, T3CH4.overCount);
        }
    }
}

// Convert float to unsigned char
void putFloat(float floatValue, unsigned char *outBuffer, uint8_t startIdx)
{
    uint8_t *p;
    p = (unsigned char *)&floatValue;
    outBuffer[startIdx] = *p;
    outBuffer[startIdx + 1] = *(p + 1);
    outBuffer[startIdx + 2] = *(p + 2);
    outBuffer[startIdx + 3] = *(p + 3);
}

// CAN filter initialization
void canFilterInit(void)
{
    if (HAL_CAN_Start(&hcan) != HAL_OK)
    {
        /* Start Error */
        Error_Handler();
    }
    if (HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK)
    {
        /* Start Error */
        Error_Handler();
    }
}

// CAN send message, takes an array pointer and data length as input parameters
// Returns 0 if data transmission is successful, returns 1 if there is a transmission error
uint8_t canSendMsg(uint8_t *msg, uint8_t len, uint32_t txID)
{
    CAN_TxHeaderTypeDef txHeader;
    uint8_t i = 0;
    uint32_t txMailbox;
    uint8_t message[8];

    txHeader.StdId = txID;                 // Set the standard identifier (11-bit)
    txHeader.IDE = CAN_ID_STD;             // Use standard frame
    txHeader.RTR = CAN_RTR_DATA;           // Set as data frame
    txHeader.DLC = len;                    // Set the data length to len
    txHeader.TransmitGlobalTime = DISABLE; // Disable the transmission time stamp

    for (i = 0; i < len; i++)
    {
        message[i] = msg[i];
    }

    if (HAL_CAN_AddTxMessage(&hcan, &txHeader, message, &txMailbox) != HAL_OK) // Add a message to the mailbox
    {
        return 1;
    }
    while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan) != 3)
    {
    }
    return 0;
}

/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
    /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1)
    {
    }
    /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
    /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
